#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>           // 用于读取里程计信息
#include <iostream>

// 全局变量
double x, y;

void leader_odom(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
  x = odomMsg->pose.pose.position.x;
  y = odomMsg->pose.pose.position.y;
  ROS_INFO("x: [%f]", odomMsg->pose.pose.position.x);
  ROS_INFO("y: [%f]", odomMsg->pose.pose.position.y);
}

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  ros::NodeHandle node;

  tf::TransformBroadcaster br, bl;
  tf::Transform transform_r, transform_l;

  //  接收领航机器人当前位置信息
  ros::Subscriber sub = node.subscribe("/tb3_0/odom", 1, leader_odom);
  ros::Rate rate(10.0);
  while (node.ok()){
    ros::spinOnce();
    transform_r.setOrigin( tf::Vector3(x - 1.0, y - 1.0, 0.0) );  // 跟随领航者右侧
    transform_r.setRotation( tf::Quaternion(0, 0, 0, 1) );
    br.sendTransform(tf::StampedTransform(transform_r, ros::Time::now(), "/map", "carrot_r"));

    transform_l.setOrigin( tf::Vector3(x - 1.0, y + 1.0 , 0.0) );   // 跟随领航者左侧
    transform_l.setRotation( tf::Quaternion(0, 0, 0, 1) );
    bl.sendTransform(tf::StampedTransform(transform_l, ros::Time::now(), "/map", "carrot_l"));

    rate.sleep();
  }
  return 0;
};